#pragma config(Sensor, in1,    armPot,              sensorPotentiometer)
#pragma config(Sensor, in2,    clawPot,             sensorPotentiometer)
#pragma config(Sensor, in3,    lineFollower,        sensorLineFollower)
#pragma config(Sensor, dgtl1,  rightEnc,            sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  leftEnc,             sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  sonar,               sensorSONAR_cm)
#pragma config(Sensor, dgtl10, rLight,              sensorDigitalOut)
#pragma config(Sensor, dgtl11, yLight,              sensorDigitalOut)
#pragma config(Sensor, dgtl12, gLight,              sensorDigitalOut)
#pragma config(Motor,  port1,           arm,           tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port2,           right,         tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port3,           left,          tmotorNormal, openLoop)
#pragma config(Motor,  port6,           right2,        tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port7,           left2,         tmotorNormal, openLoop)
#pragma config(Motor,  port8,           claw,          tmotorNormal, openLoop)
#pragma config(Motor,  port10,          arm2,          tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(0)
#pragma userControlDuration(999999)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!
//
// Global variables
int cpStart=600;
int arm_top_goal_high = 3289;
int arm_top_goal_low = 2900;
int arm_low_goal = 3006;
int arm_base = 2470;
int arm_floor = 2220;
int claw_open = 1348;
int claw_closed = 1750; // was 1800
int low_power = 32;
int mid_power = 64;
int high_power = 127;

//
// Forward declaration of functions
//
void DriveTime(int drive_time, int drive_power, int counts);
void move(int left_power, int right_power, int wait_time);
void MoveClaw(int power, int final_pos);
void MoveArm(int power, int final_pos);
void TurnTime(int drive_time, int drive_power, int counts);

int inchToClick(float inch_val)
{ float click_to_inch = 10.0;
  return (inch_val * click_to_inch);
}
/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{
   cpStart=SensorValue(clawPot);
   cpStart = 600; // Set to 600 as initial value
   SensorValue(rightEnc)=1;
   SensorValue(leftEnc)=1;
   SensorValue(rLight)=1;
	// All activities that occur before the competition starts
	// Example: clearing encoders, setting servo positions, ...
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////
void test_autonomous();
task autonomous()
{
  test_autonomous();
}
void test_autonomous()
{
  // .....................................................................................
  // Insert user code here.
  // .....................................................................................
 /* DriveTime(50, -mid_power, inchToClick(0.25));//
  wait1Msec(500);*/
  DriveTime(400, -mid_power, inchToClick(0.5));//drive backwards?
  wait1Msec(400);
  DriveTime(500, mid_power, inchToClick(0.2));//drive backwards?
  wait1Msec(400);

  MoveClaw(mid_power, claw_closed);//close the claw on the pre-load
  wait1Msec(1000);
  DriveTime(200, -mid_power, inchToClick(4.5));// drive backward?
    wait1Msec(1000);
  MoveArm((mid_power*1.5), arm_top_goal_high);// lift arm to high goal height
    wait1Msec(1000);
  DriveTime(200, (mid_power - 10), inchToClick( 5.0));// drive forward to put arm over goal
    wait1Msec(1000);
  MoveArm(0, arm_top_goal_low);// lower arm
    wait1Msec(1000);
  motor[arm] = low_power;
  motor[arm2] = low_power;
  wait1Msec(300);
  motor[arm] = 0;
  motor[arm2] = 0;
  MoveClaw(mid_power, claw_open);
  DriveTime(200, -mid_power, inchToClick( 12.0));// back up
  MoveClaw(mid_power, claw_closed);
  MoveArm(low_power, arm_floor);
  MoveClaw(low_power,claw_open);// get stack
  TurnTime(450, -mid_power, -1);
  DriveTime(200, (mid_power*0.8), inchToClick( 8.0));
  MoveClaw(high_power, claw_closed);
  MoveArm(low_power, arm_low_goal);
  TurnTime(100, mid_power, -1);
  /*DriveTime(200, mid_power, inchToClick( 16.0));
  MoveArm(mid_power,arm_base);
  */

//	AuonomousCodePlaceholderForTesting();  // Remove this function call once you have "real" code.
}
void updateLights()
{
  // Green -- Claw closed
  if (SensorValue(clawPot)+ 100 > claw_closed) SensorValue(gLight) = 0;
  else SensorValue(gLight) = 1;
  // Yellow -- Arm bove low goal
  if (SensorValue(armPot) > arm_low_goal) SensorValue(yLight) = 0;
  else SensorValue(yLight) = 1;
  if (SensorValue(armPot) < arm_base) SensorValue(rLight) = 0;
  else SensorValue(rLight) = 1;
}

int armscale(int input)        // for arm
{
  if(input<0)
    return .5*input;
  if(input>=0)
    return 1*input;
  return 0;
}
int clawScale(int limit_switch)        // for claw
{
  int c=SensorValue(clawPot);
  int min_rotation = 800;
  if((c<=cpStart+min_rotation) && (limit_switch > 0))
    return 0;
 else
    return -63;
}
int scale(int input)          // for drive train
{
  if(input<=96&&input>=-96)     //if between -96 and 96, run half power
    return .5*input;
  if(input<-96)                 // if less than -96, run at (80/32)-48 power ---> full power
    return 80/32*input-48;
  if(input>96)
    return 79/31*input+48;      // if greater than 96, run at (79/32)+48 power ---> full power
  return 0;
}
int scale2(int input)          // for drive train scale v2
{
  int da=32;// declare input changes
  int db=64;
  int dc=127;
  int ma=.2;// declare multipliers
  int mb=.35;
  int mc=(dc-(ma*da+db*mb))/(dc-db);
  if(input<=da&&input>=-da)     //if between -1st benchmark and 1st benchmark, run lowest multiplier
    return ma*input;
  if(input>=-db&&input<-da)                 // -1st>input>=-2nd, run at (.7)-32 power
    return mb*input-da*ma;
  if(input<=db&&input>da)                 // 64<input<=96, run at (.7)+32 power
    return mb*input+da*ma;
  if(input<-db)                 // if less than -96, run at (29/32)-99 power ---> ful/////l power
    return mc*input-da*ma-db*mb;
  if(input>db)                  // if greater than 96, run at (28/31)+99 power ---> full power
    return mc*input+da*ma+db*mb;
  return 0;
}
task usercontrol()
{
	// User control code here, inside the loop

	while (true)
	{
	  /*right- port 2,6
	  left- port 3,7
	  claw- port 8
	  arm- port 1,10*/

    motor[port2] = scale2(vexRT[Ch3] - vexRT[Ch4]);        // arcade drive
    motor[port6] = scale2(vexRT[Ch3] - vexRT[Ch4]);
    motor[port3] = scale2(vexRT[Ch3] + vexRT[Ch4]);
    motor[port7] = scale2(vexRT[Ch3] + vexRT[Ch4]);

    //Arm left joystick ch3
    motor[port1]=armscale(vexRT[Ch2]);
    motor[port10]=armscale(vexRT[Ch2]);
    int limit_pressed = (vexRT[Btn6D] != 0);
    //Claw open (port 8)ch6
    if (vexRT[Btn6U] != 0 ||vexRT[Btn5U] != 0 )
      motor[port8]= 127;
    if (vexRT[Btn6D] != 0||vexRT[Btn5D] != 0 )
      motor[port8] = clawScale(limit_pressed);
    else if( vexRT[Btn6U] == 0&&vexRT[Btn5U] == 0 )
      motor[port8] = 0;
    if (vexRT[Btn7U] != 0 && time1[T2] > 1000) {
       DriveTime(500, 32, 120);
       ClearTimer(T2);
    }
    if (vexRT[Btn7D] != 0 && time1[T2] > 1000) {
      DriveTime(500, -32, 120);
      ClearTimer(T2);
    }
    //
    // Update Lights
    //
    updateLights();
	}
}
void move(int left_power,int right_power,int wait_time)		//Function definition for move
{
	motor[left] = left_power;		   //Motor on left is set to power level specified in left_power parameter
  motor[left2] = left_power;
	motor[right] = right_power;			//Motor on right is set to power level specified in right_power parameter
  motor[right2] = right_power;
	if (wait_time > 0) wait1Msec(wait_time);	//Waits the number of milliseconds specified in the wait_time parameter
}
void DriveTimeSmall(int drive_time, int drive_power, int counts)
{

  float slow_factor = 0.9;
  int start_enc, end_enc;
  int check_counts=0;
  if(counts>0) check_counts = 1;
  SensorValue(rightEnc)=0;
  SensorValue(leftEnc)=0;
  ClearTimer(T1);
  end_enc = SensorValue(rightEnc);
  while ((check_counts && (abs(end_enc) < counts)) || (!check_counts &&time1[T1] < drive_time)) {

    if (SensorValue(leftEnc) == SensorValue(rightEnc)) {
      move(drive_power, drive_power, 0);
    } else if (SensorValue(leftEnc) > SensorValue(rightEnc)) {
      move ((int)drive_power*slow_factor,drive_power,0);
    } else {
      move (drive_power, (int)drive_power*slow_factor,0);
    }
    end_enc = SensorValue(rightEnc);
  }

}
void DriveTime(int drive_time, int drive_power, int counts)
{
  int block_size = 100;
  if (counts > block_size) {
    int batch_count = counts/block_size;
    for (int i = 0; i < batch_count; i++) {
      int counts_left = counts - i*block_size;
      int small_count = counts_left;
      if (small_count > block_size) small_count = block_size;
      DriveTimeSmall(drive_time, drive_power, counts);
    }
  } else {
    DriveTimeSmall(drive_time, drive_power, counts);
  }
  move(0,0,0);
}
void TurnTime(int drive_time, int drive_power, int counts)
{
  int end_enc = 0;
  int check_counts=0;
  if (counts > 0) check_counts=1;
  ClearTimer(T1);
  end_enc = SensorValue(rightEnc);
  while ((check_counts && (abs(end_enc) < counts)) || (!check_counts &&time1[T1] < drive_time)) {
    move (-drive_power,drive_power,0);
  }
}
void MovePot(int power, int final_pos, int my_pot, int motor1, int motor2)
{
  int current_pos = SensorValue(my_pot);
  int arm_power = power;
  float arm_factor;
  int arm_diff = abs(current_pos - final_pos);
  while(arm_diff > 50){
    if (current_pos > final_pos){
      arm_factor = 1.0;
    }
    else if(current_pos < final_pos){
      arm_factor = -1;
    }
    else{
      arm_factor = 0;
    }
    motor[motor1] = arm_factor * arm_power;
    if (motor2 >= 0) motor[motor2] = arm_factor * arm_power;
    current_pos = SensorValue(my_pot);
    arm_diff = abs(current_pos - final_pos);
  }
}
void MoveClaw(int power, int final_pos)
{
  MovePot(-power, final_pos, clawPot, claw, -1);


}
void MoveArm(int power, int final_pos)
{
  MovePot(-power, final_pos, armPot, arm, arm2);
}
